//
// Created by xiang on 2022/3/15.
//
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/highgui.hpp>

#include "ch6/lidar_2d_utils.h"
#include "common/io_utils.h"

DEFINE_string(file_path, "/home/zl/Public/kaist_data/urban_data/", "数据文件路径");
DEFINE_string(txt_path, "/home/zl/Public/kaist_data/urban_data/sensor_data/data_stamp.csv", "数据文件路径");
/// 测试从rosbag中读取2d scan并plot的结果
int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    sad::KaistCsvIO io(FLAGS_file_path, FLAGS_txt_path);
    io.SetScan2DProcessFunc([&](const Scan2d& scan) {
        cv::Mat image;
        Scan2d scan2d = scan;
        sad::Visualize2DScan(std::make_shared<Scan2d>(scan2d), SE2(), image, Vec3b(0, 0, 255));
        cv::imshow("scan", image);
        cv::waitKey(20);
        return true;

    }).Go();

    return 0;
}